#include "jarvis.h"
#include <iostream>

#define ERROR 0.00001

Jarvis::Jarvis() : Methods(){
}

Jarvis::~Jarvis(){
}

Face *Jarvis::calculate(Object *_obj, std::vector<Vertex *> _vertices, Point _ref, Vertex *_v){
    Vertex *prox = _v, *current, *currentV = _v;
    double maxCosAlpha = -2; //com isso teremos o menor angulo alfa e o maior beta como queriamos
    double currentCosAlpha;
    Point currentVector;
    Point proxVector;


    Face *f = new Face(faceID++, _obj, true);
    _obj->addFace(f);
    Edge *e;
    Edge *prevE = NULL;
    bool wise, wisePrevE = true;
    bool connectedToV = false;
    unsigned int proxI = 0;


    do{
        maxCosAlpha = -2;
        //testar contra todos
        for(unsigned int i = 0; i < _vertices.size(); i++){
            current = _vertices.at(i);

            currentVector = current->getPoint() - currentV->getPoint();
            currentCosAlpha = cosAlpha(_ref.normalization(), currentVector.normalization());


            //comparar os angulos.... se achar um maior apaga o vetor e adiciona, se achar um igual somente adiciona
            if ((currentCosAlpha >= maxCosAlpha - ERROR) && (currentCosAlpha <= maxCosAlpha + ERROR)){//igual
                if (currentVector.length() > proxVector.length()){
                    maxCosAlpha = currentCosAlpha;
                    prox = current;
                    proxVector = currentVector;
                    proxI = i;
                }
            }else if(currentCosAlpha > maxCosAlpha){
                maxCosAlpha = currentCosAlpha;
                prox = current;
                proxVector = currentVector;
                proxI = i;
            }
        }
        //testar contra v
        if(currentV != _v){
            current = _v;

            currentVector = current->getPoint() - currentV->getPoint();
            currentCosAlpha = cosAlpha(_ref.normalization(), currentVector.normalization());

            //comparar os angulos.... se achar um maior apaga o vetor e adiciona, se achar um igual somente adiciona
            if ((currentCosAlpha >= maxCosAlpha - ERROR) && (currentCosAlpha <= maxCosAlpha + ERROR)){//igual
                if (currentVector.length() > proxVector.length()){
                    maxCosAlpha = currentCosAlpha;
                    prox = current;
                    proxVector = currentVector;
                    _vertices.clear();
                    connectedToV = true;
                }
            }else if(currentCosAlpha > maxCosAlpha){
                maxCosAlpha = currentCosAlpha;
                prox = current;
                proxVector = currentVector;
                _vertices.clear();
                connectedToV = true;
            }
        }

        //tendo achado o proximo ponto, temos que liga-lo
        e = new Edge(edgeID++, currentV, prox, true);
        prox->setIsBoundary(true);
        wise = _obj->addEdge(&e);
        if(e->getID() != edgeID - 1) edgeID = edgeID - 1;

        currentV->setEdge(e, wise);
        if(currentV == _v){//primeira vez
            f->setEdge(e, wise);
            if(wise){
                e->setClockwise(f);
            }else{
                e->setCounterClockwise(f);
            }
        }else{
            if(wise){
                e->setClockwise(f);
                e->setPreviousClockwise(prevE, wisePrevE);

                if(wisePrevE){
                    prevE->setNextClockwise(e, wise);
                }else{
                    prevE->setNextCounterClockwise(e, wise);
                }
            }else{
                e->setCounterClockwise(f);
                e->setPreviousCounterClockwise(prevE, wisePrevE);

                if(wisePrevE){
                    prevE->setNextClockwise(e, wise);
                }else{
                    prevE->setNextCounterClockwise(e, wise);
                }
            }
        }

        prevE = e;
        wisePrevE = wise;


        if(!_vertices.empty()) _vertices.erase(_vertices.begin() + proxI);
        currentV = prox;
        _ref = proxVector;

    }while( !_vertices.empty() );

    //ligando a V, caso num se tenha feito isso anteriormente
    if(!connectedToV){
        e = new Edge(edgeID++, prox, _v, true);
        wise = _obj->addEdge(&e);
        if(e->getID() != edgeID - 1) edgeID = edgeID - 1;

        prox->setEdge(e, wise);
        if(wise){
            e->setClockwise(f);
            e->setPreviousClockwise(prevE, wisePrevE);
        }else{
            e->setCounterClockwise(f);
            e->setPreviousCounterClockwise(prevE, wisePrevE);
        }
        if(wisePrevE){
            prevE->setNextClockwise(e, wise);
        }else{
            prevE->setNextCounterClockwise(e, wise);
        }

        prevE = e;
        wisePrevE = wise;
    }


    //TEM QUE LIGAR A ULTIMA ARESTA A PRIMEIRA!
    e = _v->getEdge();
    wise = _v->getEdgeSignal();

    if(wise){
        e->setPreviousClockwise(prevE, wisePrevE);
    }else{
        e->setPreviousCounterClockwise(prevE, wisePrevE);
    }
    if(wisePrevE){
        prevE->setNextClockwise(e, wise);
    }else{
        prevE->setNextCounterClockwise(e, wise);
    }

    return f;
}
